#include <iostream>
#include <cstdlib>
#include <fstream>
#include <string>
#include <cstring>
#include <sys/time.h>
#include <errno.h>
#include "timetools.h"
#include <cassert>
#include "arglib.h"


using namespace std;


std::vector<double*> matricesA;
std::vector<double*> matricesB;
std::vector<double*> matricesC;

pthread_mutex_t lock;
int next_matrix = 0;
double dummy;

clarg::argInt nmats("-nm", "Number of matrix multiplications", 1);
clarg::argInt matsz("-msz", "Matrix size = N x N", 1000);
clarg::argInt nthreads("-nt", "Number of threads", 1);


double* new_matrix(double* src, unsigned N) {
  
  double *m = (double*) malloc( N*N*sizeof(double) );
  if (m == NULL) {
	printf("\n\nError: new_matrix()\n\n");
	exit( -999 );
  }

  if (src != NULL) { memcpy( m, src, N*N*sizeof(double) ); }

  else { memset( m, 0, N*N*sizeof(double) ); }
	
  return m;
}


void init_matrices(void) {
	
  for (unsigned i=0; i<nmats.get_value(); i++) {
    matricesA.push_back( new_matrix(0, matsz.get_value()) );
    matricesB.push_back( new_matrix(0, matsz.get_value()) );
    matricesC.push_back( new_matrix(0, matsz.get_value()) );
  }
}


void free_matrices() {
  
  for (unsigned i=0; i<nmats.get_value(); i++) {
    free(matricesA[i]);
    free(matricesB[i]);
    free(matricesC[i]);
  }
}


int get_matrix(void) {
	int id;
	pthread_mutex_lock(&lock);
		if ( next_matrix >= nmats.get_value() )
			id = -1;
		else
			id = next_matrix++;
	pthread_mutex_unlock(&lock);
	
	return id;
}

#ifdef MANUAL /* hand-tweaked code */

extern "C" bool __fasan_verify(const char * ptr);
extern "C" void __fasan_touch(const char * ptr);

extern "C" void fasan_safe_double(const double * ptr);

inline double safeLoadDouble(const double * ptr) { fasan_safe_double(ptr); return *ptr; }

#define SAFE_LOAD(REF) safeLoadDouble(&(REF))
#define SAFE_STORE(REF,V) { double * ptr = &(REF); fasan_safe_double(ptr); *ptr=V; }
#define VERIFY(REF) __fasan_verify((const char*) &(REF)) // true on poison
#define TOUCH(REF) __fasan_touch((const char*) &(REF)) // true on poison

void __attribute__ ((noinline)) multiply(const double * A, const double * B, double * C, unsigned N)
{
    int i=0, j=0;

    #if 0
    for (i =0; i < N*N; ++i) {
        if (VERIFY(C[i]) ||
            VERIFY(A[i]) ||
            VERIFY(B[i]))
        {
            abort();
        }
    }
    #endif

    for (i=0; i<N; ++i) {
        for (j=0; j<N; ++j) {
            SAFE_STORE(C[i*N + j], SAFE_LOAD(A[i*N + j]) + SAFE_LOAD(B[i*N + j]));
        }
    }
}


#else
void
inline
multiply(const double * A, const double * B, double * C, unsigned N)
{
    int i=0, j=0;

    for (i=0; i<N; ++i) {
        for (j=0; j<N; ++j)
            C[i*N + j] = A[i*N + j] + B[i*N + j];
    }
}
#endif

extern "C" void* worker_thread() {
	int matrix_id;

	while ( (matrix_id = get_matrix()) >= 0) {
		double* A = matricesA[matrix_id];
		double* B = matricesB[matrix_id];
		double* C = matricesC[matrix_id];
		unsigned N = matsz.get_value();

		multiply(A, B, C, N);
	}

    return NULL;
}


int main(int argc, char *argv[]) {
	if (clarg::parse_arguments(argc, argv)) {
		cerr << "Error when parsing the arguments!" << endl;
		return 1;
	}

	if (nmats.get_value() < 1) {
		cerr << "Error, nm must be >= 1" << endl;
		return 1;
	}

	init_matrices();
#ifdef ENABLE_OUTPUT
	timespec startTime, endTime;
	readTime(&startTime);
#endif
	worker_thread();
#ifdef ENABLE_OUTPUT
	readTime(&endTime);

	double elapsedTime = timeDiff(startTime, endTime);
	printf("RESULT %d\nTIME %f\n", matsz.get_value(), elapsedTime);
#endif
	free_matrices();

	return 0;
}
